#include "PID.h"
#include "MOTOR.h"
#include "board.h"
#include "stdio.h"
#include "string.h"
#include "math.h"
Pid Lpid,Rpid,AnglePid,TrackPid;			//	PID结构变量
uint8_t AngleFlag = 0;								//	角度环计算标志
volatile uint8_t TrackLight = 0x00;		//  记录循迹灯状态
float TrackError = 0;							//	标记循迹灯误差
volatile uint8_t stopFlag = 0;				// 	检查到白线停车标志位
volatile uint8_t startFlag = 0;			//	开始标志位
void PID_Init(void)
{
	Lpid.Kp = 800;
	Lpid.Ki = 10;
	Lpid.Kd = 0;
	Rpid.Kp = 800;
	Rpid.Ki = 10;
	Rpid.Kd = 0;
	Lpid.TargetValue = 0.0f;
	Rpid.TargetValue = 0.0f;
	// 在没有经过速度环调节下的角度环pid（不好用）
//	AnglePid.Kp = 1.3;
//	AnglePid.Ki = 0.5;
//	AnglePid.Kd = 0;
	
	// 原地转弯
	AnglePid.Kp = 0.005f;
	AnglePid.Ki = 0;
	AnglePid.Kd = 0.001f;
	// 带基础速度转弯
//	AnglePid.Kp = 0.02f;
//	AnglePid.Ki = 0;
//	AnglePid.Kd = 0.02f;

	TrackPid.Kp = 1.0f;
	TrackPid.Ki = 0;
	TrackPid.Kd = 0;
}

void PID_Calculate(Pid *Mpid,uint16_t ISum)
{
	// 误差项
	Mpid->Err = Mpid->TargetValue - Mpid->ActualValue;
	// 一阶滤波
//	Mpid->Err = 0.3 * Mpid->Err + 0.7 * Mpid->Err_last;
	// 积分项
	Mpid->Integral += Mpid->Err;
	if( Mpid->Integral > ISum ) Mpid->Integral = ISum;
	else if( Mpid->Integral < -ISum ) Mpid->Integral = -ISum;
	// PID计算
	Mpid->OutputValue = Mpid->Kp * Mpid->Err + \
										  Mpid->Ki * Mpid->Integral + \
										  Mpid->Kd * (Mpid->Err - Mpid->Err_last);
	// 误差传递
	Mpid->Err_last = Mpid->Err;
}
#define Test 0
#if Test == 1
char ch[70],zj[30];
#endif
float meanactualValue[2][5];
void ActualValue_MeanFilter(void)
{
	uint8_t i1,j1;
	float sum1 = 0;
	for(i1=0;i1<2;i1++){
		memmove(&meanactualValue[i1][1],&meanactualValue[i1][0],sizeof(float)*4);
	}
	// input new value
	meanactualValue[0][0] = Lpid.ActualValue;
	meanactualValue[1][0] = Rpid.ActualValue;
	// mean calculate
	for(i1=0;i1<2;i1++){
		sum1 = 0.0f;
		for(j1=0;j1<5;j1++){
			sum1 += meanactualValue[i1][j1];
		}
		if(i1 == 0)Lpid.ActualValue = sum1/5;
		else Rpid.ActualValue = sum1/5;
	}
}
void PID(void)
{
	static uint8_t AngleCount = 0;
//=================================角度环计算=================================
	if(Mode == ANGLE)
	{
		AnglePid.ActualValue = RelativelyAngle; // 以相对角度来计算
		PID_Calculate(&AnglePid,100);
		Lpid.TargetValue = -AnglePid.OutputValue;
		Rpid.TargetValue = +AnglePid.OutputValue;
		if(Lpid.TargetValue > 3.5f)Lpid.TargetValue = 3.5f;
		else if(Lpid.TargetValue < -3.5f)Lpid.TargetValue = -3.5f;
		if(Rpid.TargetValue > 3.5f)Rpid.TargetValue = 3.5f;
		else if(Rpid.TargetValue < -3.5f)Rpid.TargetValue = -3.5f;
		// 如果已经到达目标角度则退出模式
//		if( (int)AnglePid.Err >= -1 && (int)AnglePid.Err <= 1 ){
//			if(AngleCount++ >= 3)
//				Mode = STOP;
//		} else AngleCount = 0;
	}
//=================================位置环计算=================================
	else if(Mode == TRACK)//循迹位置环目标值默认为0（即期望误差为0）
	{
		TrackPid.ActualValue = TrackError;
		PID_Calculate(&TrackPid,100);
		Lpid.TargetValue = 1.5f-TrackPid.OutputValue;
		Rpid.TargetValue = 1.5f+TrackPid.OutputValue;
	}
//=================================速度环计算=================================
	Lpid.ActualValue = (float)Get_Encode(L)*100/20/13/4;
	Rpid.ActualValue = (float)Get_Encode(R)*100/20/13/4;
	//ActualValue_MeanFilter();	//实际值均值滤波
	// 进行PID运算
	PID_Calculate(&Lpid,10000);
	PID_Calculate(&Rpid,10000);
	
	// 最终控制
	MOTOR_Duty_Speed(Lpid.OutputValue,Rpid.OutputValue);
	
	/* 以下为测试使用，方便在VOFA查看波形 */
	#if Test == 1
	if (Lpid.ActualValue<0.0f)
		sprintf(ch,"-%d.%d,",(unsigned int)Lpid.ActualValue,(unsigned int)((fabs(Lpid.ActualValue-(unsigned int )Lpid.ActualValue))*1000));
	else
		sprintf(ch,"%d.%d,",(unsigned int)Lpid.ActualValue,(unsigned int)((Lpid.ActualValue-(unsigned int )Lpid.ActualValue)*1000));
		
	if (Lpid.TargetValue<0.0f)
		sprintf(zj,"-%d.%d,",(unsigned int)fabs(Lpid.TargetValue),(unsigned int)((fabs(Lpid.TargetValue-(unsigned int )fabs(Lpid.TargetValue)))*1000));
	else
		sprintf(zj,"%d.%d,",(unsigned int)Lpid.TargetValue,(unsigned int)((Lpid.TargetValue-(unsigned int )Lpid.TargetValue)*1000));
	strcat(ch,zj);
	
	if (Lpid.OutputValue<0.0f)
		sprintf(zj,"-%d.%d\n",(unsigned int)Lpid.OutputValue,(unsigned int)((fabs(Lpid.OutputValue-(unsigned int )Lpid.OutputValue))*1000));
	else
		sprintf(zj,"%d.%d\n",(unsigned int)Lpid.OutputValue,(unsigned int)((Lpid.OutputValue-(unsigned int )Lpid.OutputValue)*1000));
	strcat(ch,zj);
	
	printf(ch);
//	printf("%d\n",Lpid.OutputValue*1000);
	memset(ch,0,sizeof(ch));
	memset(zj,0,sizeof(zj));
	#endif
}
struct Key Keys[3];
void KEY_Scanf(void)
{
		Keys[0].state = DL_GPIO_readPins(KEY123_KEY1_PORT, KEY123_KEY1_PIN);
		Keys[1].state = DL_GPIO_readPins(KEY123_KEY2_PORT, KEY123_KEY2_PIN);
		Keys[2].state = DL_GPIO_readPins(KEY123_KEY3_PORT, KEY123_KEY3_PIN);
		for(unsigned char j=0;j<3;j++)
		{
			switch(Keys[j].step)
			{
				case 0:
					if(Keys[j].state == 0)Keys[j].step = 1;
					break;
				case 1:
					if(Keys[j].state == 0){
						Keys[j].step = 2;
					}
					break;
				case 2:
					if(Keys[j].state > 0){
						Keys[j].click = 1;
						Keys[j].step = 0;
					}
					else{
						if(Keys[j].kCount++ >= 100) // 长按超过1s
						{
							Keys[j].longClick = 1;
							Keys[j].kCount = 0;
							Keys[j].step = 3;
						}
					}
					break;
				case 3:
					if(Keys[j].state > 0)Keys[j].step = 0;
					break;
				default:
					Keys[j].step = 0;
					break;
			}
		}
}
volatile uint8_t BAN = 0;	//用于关闭巡线(防止遇到黑线自动巡线无法原地转弯)
void TRACK_Light_Scanf(void)
{
	static uint8_t outCount = 0;
	uint8_t OUT = 0x00;
	if( DL_GPIO_readPins(TRACK_OUT4_PORT, TRACK_OUT4_PIN) > 0 ){OUT |= 0x08;TrackError = 0; 		} // 这里一定要从小到大,因为后面的if会覆盖前面if的结果
	if( DL_GPIO_readPins(TRACK_OUT5_PORT, TRACK_OUT5_PIN) > 0 ){OUT |= 0x10;TrackError = -0.25;	}
	if( DL_GPIO_readPins(TRACK_OUT3_PORT, TRACK_OUT3_PIN) > 0 ){OUT |= 0x04;TrackError = 0.25; 	}
	if( DL_GPIO_readPins(TRACK_OUT6_PORT, TRACK_OUT6_PIN) > 0 ){OUT |= 0x20;TrackError = -0.5;	}
	if( DL_GPIO_readPins(TRACK_OUT2_PORT, TRACK_OUT2_PIN) > 0 ){OUT |= 0x02;TrackError = 0.5; 	}
	if( DL_GPIO_readPins(TRACK_OUT7_PORT, TRACK_OUT7_PIN) > 0 ){OUT |= 0x40;TrackError = -0.75;	}
	if( DL_GPIO_readPins(TRACK_OUT1_PORT, TRACK_OUT1_PIN) > 0 ){OUT |= 0x01;TrackError = 0.75; 	}
	TrackLight = OUT;
	// 判断停车点，并做一个简单的统计判决，防止意外触发
	if( OUT == 0x00 ){	// 白线
		if( Mode == TRACK ){ // 确保是巡线状态下走到最后的白边
			if( outCount++ >= 10 ){
//				if(TaskStep[3] == 1)
//					DL_GPIO_clearPins(RED_LED_PORT,RED_LED_PIN_22_PIN);
				stopFlag = 1;
				Mode = STOP;
				Lpid.TargetValue = 0;
				Rpid.TargetValue = 0;
			}
		} else {
			outCount = 0;
		}
	} else {	// 黑线
		// 有黑线就开启循迹模式
		if( Mode != TRACK && BAN == 0){
			Mode = TRACK;
		}
		outCount = 0;
	}
}

volatile uint32_t TIMG0Count = 0;
void TIMER_G0_INST_IRQHandler(void)
{
	switch( DL_TimerG_getPendingInterrupt(TIMER_G0_INST) )
	{
		case DL_TIMER_IIDX_ZERO:
			TIMG0Count++;
			KEY_Scanf();					// 按键扫描
			PID();								// PID计算
			if(startFlag == 1){
				TRACK_Light_Scanf();	// 循迹线扫描
			}
			break;
		default:
			break;
	}
}
